#include <behaviortree_cpp/bt_factory.h>
#include <behaviortree_cpp/behavior_tree.h>
#include <filesystem>
#include <fstream>
#include "rclcpp/rclcpp.hpp"
#include "SaySomething.hpp"
#include "SaySomethingWithRos.hpp"
#include "MyLifecycleNode.hpp"
#include "SaySomethingNode.hpp"

#include "test_interfaces/srv/test_bt.hpp"

using std::placeholders::_1;
using std::placeholders::_2;

/**
 * @brief  构建行为树服务，执行行为树
 */
class TestBT : public rclcpp::Node
{
private:
    std::shared_ptr<BT::Tree> tree_;

    rclcpp::CallbackGroup::SharedPtr callback_group;
    rclcpp::Service<test_interfaces::srv::TestBT>::SharedPtr service_;

public:
    TestBT(std::shared_ptr<BT::Tree> tree) : Node("test_bt"), tree_(tree)
    {
        // 实例化回调组, 作用为避免死锁(请自行百度ROS2死锁)
        callback_group = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);

        service_ = this->create_service<test_interfaces::srv::TestBT>("ask_bt_execute",
                                                                      std::bind(&TestBT::add_callback, this, _1, _2),
                                                                      rmw_qos_profile_services_default,
                                                                      callback_group);
    }

    ~TestBT()
    {
    }

    void add_callback(const test_interfaces::srv::TestBT::Request::SharedPtr request,
                      const test_interfaces::srv::TestBT::Response::SharedPtr response)
    {
        RCLCPP_INFO(this->get_logger(), "doing service");
        int a = request->num;

        // 执行 tick
        auto status = tree_->tickOnce();
        while (status == BT::NodeStatus::RUNNING)
        {
            tree_->sleep(std::chrono::milliseconds(100));
            status = tree_->tickOnce();
        }

        response->ret = (status == BT::NodeStatus::SUCCESS) ? 1 : 0;
        RCLCPP_INFO(this->get_logger(), "service finished %d", response->ret);
    }
};

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    rclcpp::executors::MultiThreadedExecutor executor;
    std::shared_ptr<MyLifecycleNode> my_lifecycle = std::make_shared<MyLifecycleNode>("my_lifecycle");
    my_lifecycle->auto_check(CheckType::CHECK_TO_START);

    // 2 配置行为数
    BT::BehaviorTreeFactory factory;

    // 2.1 注册你的自定义节点
    factory.registerNodeType<SaySomething>("SaySomething");
    factory.registerBuilder<SaySomethingNode>(
        "SaySomethingNode",
        [my_lifecycle](const std::string &name, const BT::NodeConfig &config)
        {
            return std::make_unique<SaySomethingNode>(name, config, my_lifecycle);
        });


    std::filesystem::path bt_xml = std::filesystem::current_path().append("src/behavior_tree/config/simple_tree_lifecycle.xml");
    BT::Tree tree = factory.createTreeFromFile(bt_xml);
    auto tree_ptr = std::make_shared<BT::Tree>(std::move(tree));
    auto test_node = std::make_shared<TestBT>(tree_ptr);

    executor.add_node(my_lifecycle->get_node_base_interface());
    executor.add_node(test_node);

    executor.spin();

    rclcpp::shutdown();
    return 0;
}
